data = load('ranging_data.txt');
%pos = load('result.txt');
%%Time pos pos pos r0 .. r 360
pos = data(:,2:4);


%Angle data
a = -180:180;
a = (a/2)*pi/180;

figure(1);
hold off;
plot(0,0,'rx');

hold on;
axis "equal";
for i=1:5:size(data,1);

% Range measurement
r = data(i,:);
r = r(5:end);
p = pos(i,:);

[v,ind]=find(r>80);
r(ind)=0;

x = r.*cos(a + p(3)) + p(1);
y = r.*sin(a + p(3)) + p(2);

plot(x,y,'.b');
plot(p(1),p(2),'rx');
sleep(0.1)
end;
